#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QMessageBox>
MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    ui->force_range->setValue(force_range);
    ui->torque_range->setValue(torque_range);

    ui->force_x_negative->setMaximum(force_range*1000);
    ui->force_x_positive->setMaximum(force_range*1000);
    ui->force_y_negative->setMaximum(force_range*1000);
    ui->force_y_positive->setMaximum(force_range*1000);
    ui->force_z_negative->setMaximum(force_range*1000);
    ui->force_z_positive->setMaximum(force_range*1000);

    ui->torque_x_negative->setMaximum(torque_range*1000);
    ui->torque_x_positive->setMaximum(torque_range*1000);
    ui->torque_y_negative->setMaximum(torque_range*1000);
    ui->torque_y_positive->setMaximum(torque_range*1000);
    ui->torque_z_negative->setMaximum(torque_range*1000);
    ui->torque_z_positive->setMaximum(torque_range*1000);

    ui->seven_axis_velocity_set->setValue(seven_axis_velocity_set);
    ui->seven_axis_absolute_position->setValue(seven_axis_position_set);
    ui->seven_axis_velocity_set_with_time->setValue(seven_axis_velocity_set_with_time);
    ui->seven_axis_velocity_set_with_current->setValue(seven_axis_velocity_set_with_current);
    ui->seven_axis_rotate_time->setValue(seven_axis_rotate_time);
    ui->seven_axis_current_limit->setValue(seven_axis_rotate_current_limet);
    ui->seven_axis_jointid->setValue(seven_axis_jointid);

    ui->force_limit->setValue(force_limit);
    ui->force_ratio->setValue(force_ratio);
    ui->drag_on_button->hide();
    ui->drag_off_button->hide();
    ui->force_sensor_zero_button->hide();
    ui->force_sensor_unzero_button->hide();

    ui->step_length->setValue(step_length);
    ui->adjust_step->setValue(adjust_step);

    ui->in_button->hide();
    ui->out_button->hide();
    ui->in_continues_button->hide();
    ui->screw_in_button->hide();
    ui->screw_out_button->hide();
    ui->screw_to_home_button->hide();
    ui->rotate_one_step->hide();
    ui->button_2->hide();
    ui->button_3->hide();

    force_data_loop = new QTimer(this);
    connect(force_data_loop, &QTimer::timeout, this, &MainWindow::force_data_loop_run);

    _tcpipGrasp=new TCPIPRobot();

    seven_axis_data_loop = new QTimer(this);
    connect(seven_axis_data_loop, &QTimer::timeout, this, &MainWindow::seven_axis_data_loop_run);

    drag_loop = new QTimer(this);
    connect(drag_loop, &QTimer::timeout, this, &MainWindow::drag_loop_run);

    usleep_no_block = new QTimer(this);
    usleep_no_block->setSingleShot(true);
    usleep_loop = new QEventLoop(this);
    connect(usleep_no_block, &QTimer::timeout, usleep_loop, &QEventLoop::quit);
}

MainWindow::~MainWindow()
{
    if (is_data_receiving)
    {
        delete force_data_receiver;
    }

    delete _tcpipGrasp;
    delete force_data_loop;
    delete drag_loop;
    delete ui;

    if (is_seven_axis_on)
    {
        seven_axis_control->off();
        delete seven_axis_control;
    }

    if (is_drag_on)
    {
        delete arm_control;
    }

    delete seven_axis_data_loop;
    delete usleep_no_block;
    delete usleep_loop;
}

void MainWindow::usleep_without_block(int us)
{
    //cout << "usleep starts!" << endl;
    usleep_no_block->start(us/1000);
    usleep_loop->exec();
    //cout << "usleep ends!" << endl;
}

void MainWindow::force_data_loop_run()
{
    for (int i=0; i<6; i++)
    {
        force_data_buffer[i] = force_data[i];
    }

    ui->force_x_value->setNum(force_data_buffer[0]);
    ui->force_y_value->setNum(force_data_buffer[1]);
    ui->force_z_value->setNum(force_data_buffer[2]);
    ui->torque_x_value->setNum(force_data_buffer[3]);
    ui->torque_y_value->setNum(force_data_buffer[4]);
    ui->torque_z_value->setNum(force_data_buffer[5]);

    if (force_data[0] >= 0)
    {
        ui->force_x_negative->setValue(0);
        ui->force_x_positive->setValue((int)(force_data_buffer[0]*1000));
    }
    else if (force_data[0] < 0)
    {
        ui->force_x_negative->setValue((int)(-force_data_buffer[0]*1000));
        ui->force_x_positive->setValue(0);
    }

    if (force_data[1] >= 0)
    {
        ui->force_y_negative->setValue(0);
        ui->force_y_positive->setValue((int)(force_data_buffer[1]*1000));
    }
    else if (force_data[1] < 0)
    {
        ui->force_y_negative->setValue((int)(-force_data_buffer[1]*1000));
        ui->force_y_positive->setValue(0);
    }

    if (force_data[2] >= 0)
    {
        ui->force_z_negative->setValue(0);
        ui->force_z_positive->setValue((int)(force_data_buffer[2]*1000));
    }
    else if (force_data[2] < 0)
    {
        ui->force_z_negative->setValue((int)(-force_data_buffer[2]*1000));
        ui->force_z_positive->setValue(0);
    }

    if (force_data[3] >= 0)
    {
        ui->torque_x_negative->setValue(0);
        ui->torque_x_positive->setValue((int)(force_data_buffer[3]*1000));
    }
    else if (force_data[3] < 0)
    {
        ui->torque_x_negative->setValue((int)(-force_data_buffer[3]*1000));
        ui->torque_x_positive->setValue(0);
    }

    if (force_data[4] >= 0)
    {
        ui->torque_y_negative->setValue(0);
        ui->torque_y_positive->setValue((int)(force_data_buffer[4]*1000));
    }
    else if (force_data[4] < 0)
    {
        ui->torque_y_negative->setValue((int)(-force_data_buffer[4]*1000));
        ui->torque_y_positive->setValue(0);
    }

    if (force_data[5] >= 0)
    {
        ui->torque_z_negative->setValue(0);
        ui->torque_z_positive->setValue((int)(force_data_buffer[5]*1000));
    }
    else if (force_data[5] < 0)
    {
        ui->torque_z_negative->setValue((int)(-force_data_buffer[5]*1000));
        ui->torque_z_positive->setValue(0);
    }
}

void MainWindow::seven_axis_data_loop_run()
{
    ui->seven_axis_current->setNum(*seven_axis_current);
    ui->seven_axis_velocity->setNum(*seven_axis_velocity);
    ui->seven_axis_position->setNum(*seven_axis_position);
    if (!is_bar_moved_by_user)
    {
        //ui->seven_axis_position_bar->setValue((int)(*seven_axis_position*100));
    }
}

void MainWindow::on_btn_GraspOpen_clicked()
{
    if(isGraspConnect>=0)
    {
        send(_tcpipGrasp->get_socket(), buffGraspOpen, RightGraspControl, 0); //发送
    }
    else
    {

    }
}

void MainWindow::on_btn_GraspClose_clicked()
{
    if(isGraspConnect>=0)
    {
        send(_tcpipGrasp->get_socket(), buffGraspClose, RightGraspControl, 0); //发送
    }
    else
    {

    }
}

void MainWindow::on_btn_data_start_clicked()
{
    force_data_receiver = new optoforce_data(force_data, force_sensor_ip, 100, 4);
    is_data_receiving = true;
    ui->force_sensor_zero_button->show();
    ui->force_sensor_unzero_button->show();
    force_data_loop->start(100);
}

void MainWindow::on_btn_data_stop_clicked()
{
    force_data_loop->stop();
    ui->force_sensor_zero_button->hide();
    ui->force_sensor_unzero_button->hide();
    delete force_data_receiver;
    is_data_receiving = false;
}

void MainWindow::on_btn_GraspConnect_clicked()
{
    isGraspConnect=_tcpipGrasp->connect("192.168.1.29",8888);

    if(isGraspConnect<0)
    {
        QMessageBox::information(this,"Right Grasp Connect","Grasp Can not be Connect!");
    }
    else
    {
        int ret=send(_tcpipGrasp->get_socket(), buffGraspFullInit, RightGraspControl, 0); //发送init
        sleep(2);
        //send(_tcpipGrasp->get_socket(), buffGraspOpen, RightGraspControl, 0); //发送close
    }
}

void MainWindow::on_force_range_valueChanged(int arg1)
{
    force_range = arg1;
    ui->force_x_negative->setMaximum(force_range*1000);
    ui->force_x_positive->setMaximum(force_range*1000);
    ui->force_y_negative->setMaximum(force_range*1000);
    ui->force_y_positive->setMaximum(force_range*1000);
    ui->force_z_negative->setMaximum(force_range*1000);
    ui->force_z_positive->setMaximum(force_range*1000);
}

void MainWindow::on_torque_range_valueChanged(int arg1)
{
    torque_range = arg1;
    ui->torque_x_negative->setMaximum(torque_range*1000);
    ui->torque_x_positive->setMaximum(torque_range*1000);
    ui->torque_y_negative->setMaximum(torque_range*1000);
    ui->torque_y_positive->setMaximum(torque_range*1000);
    ui->torque_z_negative->setMaximum(torque_range*1000);
    ui->torque_z_positive->setMaximum(torque_range*1000);
}

void MainWindow::on_btn_seven_axis_on_clicked()
{
    seven_axis_control = new single_innfos_joint("wlp1s0", 91, seven_axis_jointid);
    seven_axis_control->data_feedback(&seven_axis_position, &seven_axis_velocity, &seven_axis_current);
    seven_axis_control->on();
    seven_axis_data_loop->start(100);
    is_seven_axis_on = true;
}

void MainWindow::on_btn_seven_axis_off_clicked()
{
    seven_axis_control->off();
    seven_axis_data_loop->stop();
    delete seven_axis_control;
    is_seven_axis_on = false;
}

void MainWindow::on_btn_seven_axis_zero_clicked()
{
    seven_axis_control->zero();
}

void MainWindow::on_seven_axis_rotate_start_clicked()
{
    seven_axis_control->velcontrol_on();
    seven_axis_control->rotate(seven_axis_velocity_set);
}

void MainWindow::on_seven_axis_rotate_stop_clicked()
{
    seven_axis_control->rotate_stop();
}

void MainWindow::on_seven_axis_velocity_set_valueChanged(int arg1)
{
    seven_axis_velocity_set = arg1;
}

void MainWindow::on_seven_axis_absolute_position_valueChanged(double arg1)
{
    seven_axis_position_set = arg1;
}

void MainWindow::on_btn_seven_axis_to_absolute_pos_clicked()
{
    seven_axis_control->poscontrol_on();
    seven_axis_control->to_absolute_position(seven_axis_position_set*3.14159, false);
}

void MainWindow::on_seven_axis_velocity_set_with_time_valueChanged(int arg1)
{
    seven_axis_velocity_set_with_time = arg1;
}

void MainWindow::on_seven_axis_rotate_time_valueChanged(int arg1)
{
    seven_axis_rotate_time = arg1;
}

void MainWindow::on_seven_axis_velocity_set_with_current_valueChanged(int arg1)
{
    seven_axis_velocity_set_with_current = arg1;
}

void MainWindow::on_seven_axis_current_limit_valueChanged(double arg1)
{
    seven_axis_rotate_current_limet = arg1;
}

void MainWindow::on_seven_axis_rotate_start_with_time_clicked()
{
    seven_axis_control->velcontrol_on();
    seven_axis_control->rotate_time(seven_axis_velocity_set_with_time, seven_axis_rotate_time*1000);
}

void MainWindow::on_seven_axis_rotate_start_with_current_clicked()
{
    seven_axis_control->velcontrol_on();
    seven_axis_control->rotate_cur_limit(seven_axis_velocity_set_with_current, seven_axis_rotate_current_limet);
}

void MainWindow::on_seven_axis_jointid_valueChanged(int arg1)
{
    seven_axis_jointid = arg1;
}

void MainWindow::on_seven_axis_position_bar_valueChanged(int value)
{
    seven_axis_position_set_by_bar = value;
}

void MainWindow::on_seven_axis_position_bar_sliderPressed()
{
    is_bar_moved_by_user = true;
}

void MainWindow::on_seven_axis_position_bar_sliderReleased()
{
    seven_axis_control->poscontrol_on();
    seven_axis_control->to_absolute_position(seven_axis_position_set_by_bar/100.0, false);
    is_bar_moved_by_user = false;
}

void MainWindow::on_drag_on_button_clicked(bool checked)
{
    arm_control->setServoOn();
    drag_loop->start(500);
    is_drag_on = true;
}

void MainWindow::on_drag_on_button_toggled(bool checked)
{

}

void MainWindow::on_drag_off_button_clicked()
{
    arm_control->setServoOff();
    drag_loop->stop();
    is_drag_on = false;
}

void MainWindow::drag_loop_run()
{
    arm_control->getCurrentPoseCartSpace(arm_current_pose);
    cout << arm_current_pose[0] << ", " << arm_current_pose[1] << ", " << arm_current_pose[2] << endl;

    for (int i=0; i<6; i++)
    {
        force_data_buffer_2[i] = force_data[i];
    }
    if ((-force_data_buffer_2[1]*cos(0.349)+force_data_buffer_2[0]*sin(0.349)) > force_limit)
    {
        arm_target_pose[2] = arm_current_pose[2] + force_ratio;
        cout << "1" << endl;
    }
    else if ((-force_data_buffer_2[1]*cos(0.349)+force_data_buffer_2[0]*sin(0.349)) < -force_limit)
    {
        arm_target_pose[2] = arm_current_pose[2] - force_ratio;
        cout << "2" << endl;
    }
    else
    {
        arm_target_pose[2] = arm_current_pose[2];
    }

    if ((force_data_buffer_2[0]*cos(0.349)+force_data_buffer_2[1]*sin(0.349))  > force_limit)
    {
        arm_target_pose[0] = arm_current_pose[0] + 1*force_ratio;
        arm_target_pose[1] = arm_current_pose[1] + 0*force_ratio;
        cout << "3" << endl;
    }
    else if ((force_data_buffer_2[0]*cos(0.349)+force_data_buffer_2[1]*sin(0.349))  < -force_limit)
    {
        arm_target_pose[0] = arm_current_pose[0] - 1*force_ratio;
        arm_target_pose[1] = arm_current_pose[1] - 0*force_ratio;
        cout << "4" << endl;
    }
    else
    {
        arm_target_pose[0] = arm_current_pose[0];
        arm_target_pose[1] = arm_current_pose[1];
    }

    arm_target_pose[3] = arm_current_pose[3];
    arm_target_pose[4] = arm_current_pose[4];
    arm_target_pose[5] = arm_current_pose[5];
    arm_target_pose[6] = 0.01;
    arm_target_pose[7] = 1;

    cout << "target: " << arm_target_pose[0] << ", " << arm_target_pose[1] << ", " << arm_target_pose[2] << endl;

    arm_control->moveTaskToContinues(arm_target_pose);
}

void MainWindow::on_force_limit_valueChanged(double arg1)
{
    force_limit = arg1;
}

void MainWindow::on_force_ratio_valueChanged(double arg1)
{
    force_ratio = arg1;
}

void MainWindow::on_force_sensor_zero_button_clicked()
{
    force_data_receiver->force_sensor_doZero();
}

void MainWindow::on_force_sensor_unzero_button_clicked()
{
    force_data_receiver->force_sensor_doUnzero();
}

void MainWindow::on_arm_on_button_clicked()
{
    arm_control = new AeroRobot::AeroRobotControl("192.168.1.207");
    if (!arm_control->connectToRobot())
    {
        cout << "Cannot connect to arm!" << endl;
    }
    arm_control->setServoOn();
    is_drag_on = true;
    ui->drag_on_button->show();
    ui->drag_off_button->show();
    ui->in_button->show();
    ui->out_button->show();
    ui->in_continues_button->show();
    ui->screw_in_button->show();
    ui->screw_out_button->show();
    ui->screw_to_home_button->show();
    ui->rotate_one_step->show();
    ui->button_2->show();
    ui->button_3->show();
}

void MainWindow::on_adjust_step_valueChanged(double arg1)
{
    adjust_step = arg1;
}

void MainWindow::on_step_length_valueChanged(double arg1)
{
    step_length = arg1;
}

void MainWindow::on_in_button_clicked()
{
    is_in_button_pressed = false;
    arm_control->pMoveStop();
}

void MainWindow::on_in_button_pressed()
{
    is_in_button_pressed = true;
    for (int i=0; i<6; i++)
    {
        force_data_buffer_3[i] = force_data[i];
    }

    while(is_in_button_pressed)
    {
        arm_control->getCurrentPoseCartSpace(arm_current_pose);
        arm_end_rotate_matrix = Eigen::AngleAxisd(arm_current_pose[3], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(arm_current_pose[4], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(arm_current_pose[5], Eigen::Vector3d::UnitX());

        end_coordinate[0] = 0;
        end_coordinate[1] = 0;
        end_coordinate[2] = step_length;

        base_coordinate = arm_end_rotate_matrix * end_coordinate;

        arm_target_pose[0] = arm_current_pose[0] + base_coordinate[0];
        arm_target_pose[1] = arm_current_pose[1] + base_coordinate[1];
        arm_target_pose[2] = arm_current_pose[2] + base_coordinate[2];
        arm_target_pose[3] = arm_current_pose[3];
        arm_target_pose[4] = arm_current_pose[4];
        arm_target_pose[5] = arm_current_pose[5];
        arm_target_pose[6] = 0.01;
        arm_target_pose[7] = 1;

        arm_control->moveTaskToContinues(arm_target_pose);
        usleep_without_block(400000);
    }
}

void MainWindow::on_out_button_clicked()
{
    is_out_button_pressed = false;
    arm_control->pMoveStop();
}

void MainWindow::on_out_button_pressed()
{
    is_out_button_pressed = true;
    for (int i=0; i<6; i++)
    {
        force_data_buffer_3[i] = force_data[i];
    }

    while(is_out_button_pressed)
    {
        arm_control->getCurrentPoseCartSpace(arm_current_pose);
        arm_end_rotate_matrix = Eigen::AngleAxisd(arm_current_pose[3], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(arm_current_pose[4], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(arm_current_pose[5], Eigen::Vector3d::UnitX());

        end_coordinate[0] = 0;
        end_coordinate[1] = 0;
        end_coordinate[2] = -step_length;

        base_coordinate = arm_end_rotate_matrix * end_coordinate;

        arm_target_pose[0] = arm_current_pose[0] + base_coordinate[0];
        arm_target_pose[1] = arm_current_pose[1] + base_coordinate[1];
        arm_target_pose[2] = arm_current_pose[2] + base_coordinate[2];
        arm_target_pose[3] = arm_current_pose[3];
        arm_target_pose[4] = arm_current_pose[4];
        arm_target_pose[5] = arm_current_pose[5];
        arm_target_pose[6] = 0.01;
        arm_target_pose[7] = 1;

        arm_control->moveTaskToContinues(arm_target_pose);
        usleep_without_block(500000);
    }
}

void MainWindow::on_in_continues_button_clicked()
{
    ofstream fout;
    fout.open("data.txt", ios::out);
    fout << 1 << endl;
    fout << 2 << endl;


    for (int i=0; i<6; i++)
    {
        force_data_buffer_3[i] = force_data[i];
        fout << force_data_buffer_3[i] << " ";
    }
    fout << endl;
    while (force_data_buffer_3[2] > -50)
    {
        arm_control->getCurrentPoseCartSpace(arm_current_pose);
        arm_end_rotate_matrix = Eigen::AngleAxisd(arm_current_pose[3], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(arm_current_pose[4], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(arm_current_pose[5], Eigen::Vector3d::UnitX());

        end_coordinate[0] = 0;
        end_coordinate[1] = 0;
        end_coordinate[2] = step_length;

        base_coordinate = arm_end_rotate_matrix * end_coordinate;

        arm_target_pose[0] = arm_current_pose[0] + base_coordinate[0];
        arm_target_pose[1] = arm_current_pose[1] + base_coordinate[1];
        arm_target_pose[2] = arm_current_pose[2] + base_coordinate[2];
        arm_target_pose[3] = arm_current_pose[3];
        arm_target_pose[4] = arm_current_pose[4];
        arm_target_pose[5] = arm_current_pose[5];
        arm_target_pose[6] = 0.01;
        arm_target_pose[7] = 1;

        arm_control->moveTaskToWithSpeed(arm_target_pose);
        //cout << arm_current_pose[0] << "; " << arm_current_pose[1] << "; " << arm_current_pose[2] << endl;
        //cout << arm_target_pose[0] << "; " << arm_target_pose[1] << "; " << arm_target_pose[2] << endl;
        usleep_without_block(200000);

        for (int i=0; i<6; i++)
        {
            force_data_buffer_3[i] = force_data[i];
            fout << force_data_buffer_3[i] << " ";
        }
        fout << endl;
        cout << "after move forward: " << force_data_buffer_3[0] << ", " << force_data_buffer_3[1] << ", " << force_data_buffer_3[2] << endl;

        while(fabs(force_data_buffer_3[0])>adjust_force_limit or fabs(force_data_buffer_3[1])>adjust_force_limit)
        {

            if (fabs(force_data_buffer_3[0])>200 or fabs(force_data_buffer_3[1])>200 or fabs(force_data_buffer_3[2])>200 or fabs(force_data_buffer_3[3])>10 or fabs(force_data_buffer_3[4])>10 or fabs(force_data_buffer_3[5])>6.5)
            {
                QMessageBox::warning(NULL, "warning", "force sensor is out of range!");
                break;
                break;
            }

            if (force_data_buffer_3[2] < -80)
            {
                QMessageBox::information(NULL, "information", "Finished!");
                break;
                break;
            }

            if ((-force_data_buffer_3[1]*cos(0.349)+force_data_buffer_3[0]*sin(0.349)) > adjust_force_limit)
            {
                endpoint_adjust[2] = adjust_step;
            }
            else if ((-force_data_buffer_3[1]*cos(0.349)+force_data_buffer_3[0]*sin(0.349)) < -adjust_force_limit)
            {
                endpoint_adjust[2] = -adjust_step;
            }
            else
            {
                endpoint_adjust[2] = 0;
            }

            if ((force_data_buffer_3[0]*cos(0.349)+force_data_buffer_3[1]*sin(0.349)) > adjust_force_limit)
            {
                endpoint_adjust[0] = adjust_step;
            }
            else if ((force_data_buffer_3[0]*cos(0.349)+force_data_buffer_3[1]*sin(0.349)) < -adjust_force_limit)
            {
                endpoint_adjust[0] = -adjust_step;
            }
            else
            {
                endpoint_adjust[0] = 0;
            }

            arm_control->getCurrentPoseCartSpace(arm_current_pose);

            arm_target_pose[0] = arm_current_pose[0] + endpoint_adjust[0];
            arm_target_pose[1] = arm_current_pose[1] + endpoint_adjust[1];
            arm_target_pose[2] = arm_current_pose[2] + endpoint_adjust[2];
            arm_target_pose[3] = arm_current_pose[3];
            arm_target_pose[4] = arm_current_pose[4];
            arm_target_pose[5] = arm_current_pose[5];
            arm_target_pose[6] = 0.01;
            arm_target_pose[7] = 1;

            arm_control->moveTaskToWithSpeed(arm_target_pose);
            //cout << arm_current_pose[0] << "; " << arm_current_pose[1] << "; " << arm_current_pose[2] << endl;
            //cout << arm_target_pose[0] << "; " << arm_target_pose[1] << "; " << arm_target_pose[2] << endl;
            usleep_without_block(500000);

            for (int i=0; i<6; i++)
            {
                force_data_buffer_3[i] = force_data[i];
                fout << force_data_buffer_3[i] << " ";
            }
            fout << endl;
            cout << "after adjust: " << force_data_buffer_3[0] << ", " << force_data_buffer_3[1] << ", " << force_data_buffer_3[2] << endl;
        }

        cout << force_data_buffer_3[2] << endl;
    }
    fout.close();
    QMessageBox::information(NULL, "information", "Finished!");

}

void MainWindow::on_screw_in_button_pressed()
{
    is_screw_in_button_pressed = true;
    while (is_screw_in_button_pressed)
    {
        for (int i=0; i<6; i++)
        {
            force_data_buffer_3[i] = force_data[i];
        }
        while (force_data_buffer_3[2] > -20)
        {
            arm_control->getCurrentPoseCartSpace(arm_current_pose);
            arm_end_rotate_matrix = Eigen::AngleAxisd(arm_current_pose[3], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(arm_current_pose[4], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(arm_current_pose[5], Eigen::Vector3d::UnitX());

            end_coordinate[0] = 0;
            end_coordinate[1] = 0;
            end_coordinate[2] = step_length;

            base_coordinate = arm_end_rotate_matrix * end_coordinate;

            arm_target_pose[0] = arm_current_pose[0] + base_coordinate[0];
            arm_target_pose[1] = arm_current_pose[1] + base_coordinate[1];
            arm_target_pose[2] = arm_current_pose[2] + base_coordinate[2];
            arm_target_pose[3] = arm_current_pose[3];
            arm_target_pose[4] = arm_current_pose[4];
            arm_target_pose[5] = arm_current_pose[5];
            arm_target_pose[6] = 0.01;
            arm_target_pose[7] = 1;

            arm_control->moveTaskToWithSpeed(arm_target_pose);
            //cout << arm_current_pose[0] << "; " << arm_current_pose[1] << "; " << arm_current_pose[2] << endl;
            //cout << arm_target_pose[0] << "; " << arm_target_pose[1] << "; " << arm_target_pose[2] << endl;
            usleep_without_block(500000);
        }
        arm_control->getCurrentPoseJointSpace(arm_joint_position);
        //cout << arm_joint_position[5] << endl;
        arm_joint_position[5] = arm_joint_position[5] + 0.05;
        arm_control->moveJointTo(arm_joint_position);
        usleep_without_block(100000);
        cout << arm_joint_position[5] << endl;
    }
}

void MainWindow::on_screw_to_home_button_clicked()
{
    arm_control->getCurrentPoseJointSpace(arm_joint_position);
    arm_joint_position[5] = joint_6_home_position;
    arm_control->moveJointTo(arm_joint_position);
}

void MainWindow::on_screw_in_button_clicked()
{
    is_screw_in_button_pressed = false;
}

void MainWindow::on_screw_out_button_pressed()
{
    is_screw_out_button_pressed = true;
    while (is_screw_out_button_pressed)
    {
        for (int i=0; i<6; i++)
        {
            force_data_buffer_3[i] = force_data[i];
        }
        while (force_data_buffer_3[5] < -50)
        {
            arm_control->getCurrentPoseCartSpace(arm_current_pose);
            arm_end_rotate_matrix = Eigen::AngleAxisd(arm_current_pose[3], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(arm_current_pose[4], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(arm_current_pose[5], Eigen::Vector3d::UnitX());

            end_coordinate[0] = 0;
            end_coordinate[1] = 0;
            end_coordinate[2] = -step_length;

            base_coordinate = arm_end_rotate_matrix * end_coordinate;

            arm_target_pose[0] = arm_current_pose[0] + base_coordinate[0];
            arm_target_pose[1] = arm_current_pose[1] + base_coordinate[1];
            arm_target_pose[2] = arm_current_pose[2] + base_coordinate[2];
            arm_target_pose[3] = arm_current_pose[3];
            arm_target_pose[4] = arm_current_pose[4];
            arm_target_pose[5] = arm_current_pose[5];
            arm_target_pose[6] = 0.01;
            arm_target_pose[7] = 1;

            arm_control->moveTaskToWithSpeed(arm_target_pose);
            //cout << arm_current_pose[0] << "; " << arm_current_pose[1] << "; " << arm_current_pose[2] << endl;
            //cout << arm_target_pose[0] << "; " << arm_target_pose[1] << "; " << arm_target_pose[2] << endl;
            usleep_without_block(500000);
        }
        arm_control->getCurrentPoseJointSpace(arm_joint_position);
        //cout << arm_joint_position[5] << endl;
        arm_joint_position[5] = arm_joint_position[5] - 0.05;
        arm_control->moveJointTo(arm_joint_position);
        usleep_without_block(100000);
        cout << arm_joint_position[5] << endl;
    }
}

void MainWindow::on_screw_out_button_clicked()
{
    is_screw_out_button_pressed = false;
}

void MainWindow::on_rotate_one_step_clicked()
{
    usleep_without_block(1000000);
    force_data_receiver->force_sensor_doZero();

    double distance = 0.04;
    double task_position_velocity = 0.005;
    double step_depth = 0.001;
    double step_angle = 0.02;
    double joint_velocity = 0.1;
    double z_force_limit = 40;
    int count = 0;
    int count_finished = 3;
    double arm_pose_save[6];
    double arm_pose_save_with_speed[8];

    arm_task_move_z(distance-0.02, task_position_velocity);
    arm_control->getCurrentPoseCartSpace(arm_pose_save);


    while(1)
    {
        arm_task_move_z(0.02, task_position_velocity);
        for (int i=0; i<6; i++)
        {
            force_data_buffer_3[i] = force_data[i];
        }

        usleep_without_block(2000000);

        count = 0;
        while (fabs(force_data_buffer_3[2]) < z_force_limit)
        {
            arm_task_move_z(step_depth, task_position_velocity);
            count ++;
            for (int i=0; i<6; i++)
            {
                force_data_buffer_3[i] = force_data[i];
            }
        }
        if (count >= count_finished)
        {
            QMessageBox::information(NULL, "information", "Finished!");
            break;
        }
        else
        {
            for (int i=0; i<6; i++)
            {
                arm_pose_save_with_speed[i] = arm_pose_save[i];
            }
            arm_pose_save_with_speed[6] = task_position_velocity;
            arm_pose_save_with_speed[7] = 1;

            arm_control->moveTaskToWithSpeed(arm_pose_save);
            usleep_without_block(0.02/task_position_velocity*1000000+100000);

            arm_control->set_joint_move_speed(joint_velocity);
            arm_control->getCurrentPoseJointSpace(arm_joint_position);
            arm_joint_position[5] = arm_joint_position[5] - step_angle;
            arm_control->moveJointTo(arm_joint_position);
            usleep_without_block(step_angle/joint_velocity*1000000+100000);
            cout << arm_joint_position[5] << endl;
            arm_control->getCurrentPoseCartSpace(arm_pose_save);
        }
    }
}

void MainWindow::on_button_2_clicked()
{    

}

void MainWindow::arm_task_move_z(double distance, double velocity)
{
    arm_control->getCurrentPoseCartSpace(arm_current_pose);
    arm_end_rotate_matrix = Eigen::AngleAxisd(arm_current_pose[3], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(arm_current_pose[4], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(arm_current_pose[5], Eigen::Vector3d::UnitX());

    end_coordinate[0] = 0;
    end_coordinate[1] = 0;
    end_coordinate[2] = distance;

    base_coordinate = arm_end_rotate_matrix * end_coordinate;

    arm_target_pose[0] = arm_current_pose[0] + base_coordinate[0];
    arm_target_pose[1] = arm_current_pose[1] + base_coordinate[1];
    arm_target_pose[2] = arm_current_pose[2] + base_coordinate[2];
    arm_target_pose[3] = arm_current_pose[3];
    arm_target_pose[4] = arm_current_pose[4];
    arm_target_pose[5] = arm_current_pose[5];
    arm_target_pose[6] = velocity;
    arm_target_pose[7] = 1;

    arm_control->moveTaskToContinues(arm_target_pose);
    usleep_without_block(distance/velocity*1000000+100000);
}
